package org.maxgroundstation.simulation.service.video;

import java.awt.Color;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.image.BufferedImage;
import java.io.ByteArrayOutputStream;
import java.util.Iterator;
import java.util.concurrent.ScheduledThreadPoolExecutor;
import java.util.concurrent.TimeUnit;

import javax.imageio.ImageIO;

import org.maxgroundstation.simulation.service.Service;
import org.maxgroundstation.simulation.service.ServiceCommunication;
import org.maxgroundstation.simulation.service.ServiceDataFrame;
import org.maxgroundstation.simulation.service.ServiceSubscriber;
import org.maxgroundstation.world.WorldModel;

/**
 * The video service.
 */
public class VideoService extends Service {
	
	// Data members
    private String id;
	private Color foreground;
	private Color background;
	private int imageWidth;
	private int imageHeight;
	private int fps;
	private double imageRotation;
	private ScheduledThreadPoolExecutor executor;
	
	/**
	 * Constructor
	 * @param comm the service communication.
	 * @param id the service ID.
	 * @param name the service name.
	 * @param foreground the foreground color.
	 * @param background the background color.
	 */
	public VideoService(ServiceCommunication comm, String id, String name, 
	        Color foreground, Color background) {
		// Use Service constructor.
		super(comm);
		
		// Initialize data members.
		this.id = id;
		this.name = name;
		this.foreground = foreground;
		this.background = background;
		description = "Returns video frames from camera when subscribed to.";
		imageRotation = 0D;
		
		// Set initial video parameters.
		imageWidth = 320;
		imageHeight = 240;
		fps = 30;
		
		// Start video thread execution.
		startExecution();
	}
	
	/**
	 * Starts executing the video service.
	 */
	private void startExecution() {
		if (executor != null) executor.shutdownNow();
		
		long period = 1000L;
    	if (fps != 0) period /= fps;
		
		executor = new ScheduledThreadPoolExecutor(1);
		executor.scheduleAtFixedRate(new Runnable() {
			@Override
			public void run() {
				// left = generated graphic, right = simulated scene
     	        byte[] pictureBytes = null;
				try {
					pictureBytes = (getID().startsWith("lvid"))
					  ? compressVideoImage(generateImage()) : renderScenery();
				} catch (Exception e1) {
					e1.printStackTrace();
				}

     	        if(pictureBytes.equals(null)){
     	        	System.out.printf("Draw a blank\n");
     	        }
		        // Convert image data to jpg byte array.
		        try {
		            sendImageBytes(pictureBytes);
		        }
		        catch (Exception e) {
		        	//comm.log(e);
		        }
			}		
		}, 0, period, TimeUnit.MILLISECONDS);
	}
	
	@Override
	public String getID() {
		return id;
	}

	@Override
	public boolean isDefaultService() {
		return false;
	}

	/**
	 * Gets the current video image width.
	 * @return video image width.
	 */
	public int getVideoImageWidth() {
		return imageWidth;
	}
	
	/**
	 * Sets the video image width.
	 * @param imageWidth the video image width (in pixels).
	 */
	public void setVideoImageWidth(int imageWidth) {
		this.imageWidth = imageWidth;
	}
	
	/**
	 * Gets the current video image height.
	 * @return video image height.
	 */
	public int getVideoImageHeight() {
		return imageHeight;
	}
	
	/**
	 * Sets the video image height.
	 * @param imageHeight the video image height (in pixels).
	 */
	public void setVideoImageHeight(int imageHeight) {
		this.imageHeight = imageHeight;
	}
	
	/**
	 * Gets the current video frames per second.
	 * @return video frames per second.
	 */
	public int getFPS() {
		return fps;
	}
	
	/**
	 * Sets the video frames per second.
	 * @param fps video frames per second.
	 */
	public void setFPS(int fps) {
		if (fps != this.fps) {
			this.fps = fps;

			// Start video thread execution with new period.
			startExecution();
		}
	}

	@Override
	public void handleResponse(ServiceDataFrame dataFrame, int commType)
			throws Exception {
		// Do nothing.
	}

	@Override
	public void shutDown() {
		executor.shutdownNow();
	}
	
	@Override
	public boolean isSubscribable() {
		return true;
	}
    	
	/** 
	 * Generates the video image.
	 * 
     * @return a buffered image.
     */
    private BufferedImage generateImage() {
    	
    	if (fps > 0) {
        	double twoPI = Math.PI * 2D;
        	imageRotation += twoPI / fps / 3D;
        	if (imageRotation > twoPI) imageRotation -= twoPI;
        }
    	
    	BufferedImage result = new BufferedImage(imageWidth, imageHeight, BufferedImage.TYPE_INT_RGB);
    	
    	Graphics2D graphics = result.createGraphics();
    	graphics.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
    	
    	graphics.setBackground(background);
    	graphics.clearRect(0, 0, imageWidth, imageHeight);
    	graphics.setColor(foreground);
    	
    	graphics.rotate(imageRotation, (imageWidth / 2), (imageHeight / 2));
    	
    	//System.out.println("Generating image rotation: " + imageRotation + "for fps: " + fps);
    	
    	int ovalWidth = imageWidth / 2;
    	int ovalHeight = imageHeight / 4;
    	
    	int x = (imageWidth / 2) - (ovalWidth / 2);
    	int y = (imageHeight / 2) - (ovalHeight / 2);
    	graphics.drawOval(x, y, ovalWidth, ovalHeight);
    	
    	return result;
    }
    
    /**
     * Compresses video image to be sent to all subscribers.
     * @param image the video image.
     * @throws Exception if error sending video image.
     */
    private byte[] compressVideoImage(BufferedImage image) throws Exception {
    	if(image==null)return null;
    	ByteArrayOutputStream baos = new ByteArrayOutputStream();
    	ImageIO.write(image, "jpg", baos);
    	byte[] imageData = baos.toByteArray();
    	return imageData;
    }
   	
    /**
     * Send the compresses image to all subscribers.
     * @param imageData e.g.: the return from compressVideoImage()
     * @throws Exception
     */
    private void sendImageBytes(byte[] imageData) throws Exception {
    	if(imageData==null)return;
    	Iterator<ServiceSubscriber> i = subscribers.iterator();
    	while (i.hasNext()) {
    		ServiceSubscriber subscriber = i.next();
    		if (comm.getClientManager().checkClient(subscriber.getClientID())) {
    			try {
    				comm.sendServicePost(subscriber.getCommType(), id, 
    						subscriber.getClientID(), imageData);
    			}
    			catch (Exception e) {
    				comm.log(e);
    			}
    		}
    		else i.remove();
    	}
    }    

/*****************************************************************************\
*																			  *
*	The following code was added by Judd Reed beginning on July 1, 2011.  	  *
*	It adds scenery to the video stream from the simulated rover based on	  *
*	elevation and surface data from public GIS data portals.				  *	
*																			  *
\*****************************************************************************/

	private int napTime = 0;
	private double wasLon=0, wasLat=0, wasHead=0;
	public int stillCount(double[] lonLatEle, double[] heading) {
		boolean still = true;
		if(wasLon!=lonLatEle[0])still=false;
		if(wasLat!=lonLatEle[1])still=false;
		if(wasHead!=heading[0])still=false;
		if(still)
			napTime++;
		else {
			napTime=0;
			wasLon = lonLatEle[0];
			wasLat = lonLatEle[1];
			wasHead =heading[0];
		}
		return( napTime );
	}
	
	private WorldModel gisData = null;
	
	//TODO: Should this be a command line argument or come from a setup file?
	private final String modelOfTheWorld = "resources/metaData.txt";

	private byte[] previousSceneBytes = null;
	private int wasWide=0;
    private byte[] renderScenery() {
    	if(gisData == null) {
    		System.out.printf("LOAD %s\n", modelOfTheWorld);
    		gisData = new WorldModel(modelOfTheWorld);

    		// place robot at scene center upon initial setup
    		getServiceCommunication().getRobot().setLatitude(gisData.getReferenceLatitude());
    		getServiceCommunication().getRobot().setLongitude(gisData.getReferenceLongitude());
    	}
    	
        BufferedImage result = new BufferedImage(imageWidth, imageHeight, BufferedImage.TYPE_INT_RGB);
    	Graphics2D g2 = result.createGraphics();		  
        int wide = result.getWidth();
        int high = result.getHeight();
        double viewWide = 45.0;
        double focalLengthInPixels = (double)wide * 0.5 / Math.tan(0.5*viewWide*Math.PI/180.0);
    	CameraSituation pointOfView = new CameraSituation(getServiceCommunication().getRobot());
    	double[] xyz = pointOfView.getRobotLocation();
    	double cameraHeight = xyz[2] = gisData.getHigh(xyz[0], xyz[1], true);
    	pointOfView.informElevation(xyz[2]);
    	pointOfView.moveToCamera(xyz);
    	cameraHeight -= xyz[2];// now this is the negative distance from ground to camera
       	double[] look = pointOfView.getLookingDirection();
       	int stillness = stillCount(xyz, look);
       	/*PSEUDOCODE:
       	 * If moving
       	 *   render with nearest neighbor colors and bilinear elevation
       	 *   I tried nearest neighbor elevation (where it matters most) but it was hideous.
       	 * If still
       	 *   render with bilinear resampling in surface rg&b as well as elevation
       	 * if still still
       	 *   resend the earlier picture.
       	 */
       	if((stillness>1)&&(wide==wasWide)) {
       		return previousSceneBytes;
       	}
       	
       	wasWide = wide;

      	for(int x=0;x<wide;x++) {
    		int hor = high;
    		double dOdI = (double)(x-wide/2)/(double)focalLengthInPixels;
    		// start at depth into scene 1/2 way to first visible spot on level ground
	    	double into = -cameraHeight*focalLengthInPixels/(double)high;
	    	double delta = 1.0 + into / focalLengthInPixels;
	    	double dEdI = (look[0] + dOdI*look[1]);
	    	double dNdI = (look[1] - dOdI*look[0]);
	    	for(;true;into *= delta) {// sample squarely: bigger steps as depth increases
		    	double east = xyz[0] + into*dEdI;	
	    		double nrth = xyz[1] + into*dNdI;
	    		double slope = gisData.getHigh(east, nrth, true );
	    		if(slope<=0.0) break;
	    		
    			int y = Math.max(0, high/2-(int)(focalLengthInPixels*(slope-xyz[2])/into) );
    			if(hor > y) {
    				g2.setColor(new Color(gisData.getSurface(east, nrth, true)));//(stillness>0) )));					
    				g2.drawLine(x,y,x,hor);
    				hor = y;
			}	}
	    	if(hor > 0) {
	    		//TODO(jr) choose any of these:
	    		//int skyPixel = 0x0ffcc88;//a nice rust orange for mars sky
	    		//int skyPixel = 0x088ccff;//a nice light blue for earthly sky
	    		// sky is pale blue in the south and deeper blue to the north
	    		int skyPixel = 0x0ffffff - 0x0030200 * (int)(20.0 * (1.5 + look[1] - dOdI*look[0]));
	    		g2.setColor(new Color(skyPixel));
	    		g2.drawLine(x,0,x,hor);
    	}	}
      	try {
			previousSceneBytes = compressVideoImage(result);
		} catch (Exception e) {
			e.printStackTrace();
		}
   		return previousSceneBytes;
    }
	// end of code added by Jud
}